import os.path

import pandas as pd
from matplotlib import pyplot as plt
from Algorithm.AlgorithmModel import ControlResult, ControlMode
from Algorithm.M_BaseServo import BaseServo
from Algorithm.AdmittnceMNRControl import AddmittanceControl
from Algorithm.AdaptiveAdmittanceControl import AdaptiveAdmittanceControl
import numpy as np
import cv2
import Common.MatrixProcess as mp

from Common.ControlEnum import forcemode as fmode
from Common.CreateFolder import FileNameGenerator, FolderManager
from Common.MatrixProcess import trans_pose
class my_test(BaseServo):
    def __init__(self,initPose,args,dt=0.002,mode=fmode.FreeMode):
        # democsv="result/demotraj/demo1.csv"
        super(my_test,self).__init__(ControlMode=ControlMode.servoL)
        root=args["demo_path_root"]
        democsv= os.path.join(args["replay_folder"],"teach.csv")#复现模式的轨迹
        self.controller=AddmittanceControl(M6=np.array([5,5,10,20,20,2]),
                                           B6=np.array([600,600,800,50,50,10]),
                                           K6=np.array([0,0,0,0,0,0]),
                                           FT6=np.array([0,0,25,0,0,0]),
                                           dt=0.002,
                                           teachmode=mode,
                                           democsv=democsv)
        print(self.result)
        self.test_path(initPose,step=dt)
        self.mode=mode
        if(mode==fmode.TeachMode):
            #创建文件夹
            # target_directory = root  # Replace this with the desired target directory
            # folder_manager = FolderManager(args["teach_folder"], target_directory)
            # self.saveFolder = folder_manager.create_new_folder()
            self.saveFile=os.path.join(args["save_folder"],"teach.csv")
        elif(mode==fmode.ReplayMode):
            # self.replayFolder=os.path.join(root,args["replay_folder"])
            self.is_replay_record=args["is_replay_record"]
            if(self.is_replay_record==True):
                self.replay_record_file=os.path.join(args["replay_folder"],"replay.csv")

        # self.savelist=[]
        #根据示教的文件编号，生成复现的文件编号
        # num=self.__extract_string(args["teachdemo"])
        # self.replayfile="replaydemo"+num+".csv"
        # self.replayfile=os.path.join(root,self.replayfile)
        # print(f"保存文件路径：{self.saveFile,self.replayfile}")

    def test_path(self,initPose,step=0.002,path=0.2,target_v =0,target_a = 0):
        """
        螺旋寻孔,最大圆半径=incrR*num_turns * 2 * np.pi
        Args:
            initPose:当前机器人位置
            startR:起始R
            incrR:每个时刻半径增量，每圈增量=incrR*points,大圆直径=incrR*points/10mm
            num_turns:多少圈
            points:每圈多少个点
        Returns:
        """
        self.initPose = initPose
        # self.z = np.arange(self.initPose[2],self.initPose[2]-path,-step)#速度一定
        # self.maxcount = self.z.size
        self.maxcount = 500
        self.z = np.zeros(self.maxcount)
        self.target_v = target_v  # v = 0.01m/s
        i=0
        while(i<self.maxcount):
            # self.z[i] = self.initPose[2] - step*step*i*i*target_a      #根据加速度设置xin,x=1/2* a *t^2
            self.z[i] = self.initPose[2] - step * i * self.target_v - step*step*i*i*target_a    #step为ur5e的控制周期 2ms
            i=i+1
        self._curPoseCount = 0

        # self.maxcount=20000

    def Move(self, **kwargs):
        cur_pose= kwargs['actualTcpPose']
        cur_vel = kwargs['actualTcpSpeed']
        cur_force=kwargs['force']
        pose=self.initPose
        pose[2]=self.z[self._curPoseCount]
        self._curPoseCount+=1
        pose=self.controller.calpose(cur_pose,pose,cur_force)
        self.result['ServoParm'][0] = pose


        if(self._curPoseCount==self.maxcount-1):
            self.result['ControlResult'] = ControlResult.Finish
            if (self.mode == fmode.TeachMode):
                data = self.controller.get_save_value()
                df=pd.DataFrame({
                    'force':list(data["BaseForce"]),
                    'pose': list(data["actualTcpPose"]),
                })
                df.to_csv(self.saveFile,index=False)
            if(self.mode==fmode.ReplayMode):
                if(self.is_replay_record==True):
                    data = self.controller.get_save_value()
                    df=pd.DataFrame({
                        'force':list(data["BaseForce"]),
                        'pose': list(data["actualTcpPose"]),
                        'camera': list(data["muti_sensor_timestamp"]),
                    })
                    df.to_csv(self.replay_record_file, index=False)

        elif(np.linalg.norm(cur_force)>80):
            self.result['ControlResult'] = ControlResult.Finish
            print("超出最大力阈值",cur_force)
        else:
            self.result['ControlResult'] = ControlResult.Running
        return self.result

    def __extract_string(self,filename):
        # 查找 "demo" 的起始位置
        start_index = filename.find("demo")
        if start_index == -1:
            return None

        # 从 "demo" 后面的字符开始查找 "." 的位置
        start_index += len("demo")
        end_index = filename.find(".", start_index)

        # 如果找到 "."，提取 demo 和 . 之间的子字符串
        if end_index != -1:
            return filename[start_index:end_index]
        else:
            return None
